#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander

class MoveItFkDemo:
    def __init__(self):

        # 初始化Python API 依赖的moveit_commanderC++系统，需放在前面
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点，节点名为'moveit_fk_demo'
        rospy.init_node('moveit_fk_demo', anonymous=True)

        # 初始化需要控制的规划组。
        # 规划组已在配置助手setup assistant里进行了配置，并保存到了SRDF文件中，只需传入组名即可。
        arm = moveit_commander.MoveGroupCommander('xarm')
        gripper = moveit_commander.MoveGroupCommander('gripper')

        # 设置机械臂的目标位姿，使用xarm组六个关节的位置数据进行描述（单位：弧度），每个位置对应着一个关节。
        # 六个关节全为0时是机械臂的初始位置
        joint_positions = [0, 0, 0, 0, 0, 0]
        arm.set_joint_value_target(joint_positions)

        # 规划出一条从当前位姿到目标位姿的轨迹，存放在traj变量里
        # plan_success, traj, planning_time, error_code = arm.plan() # noetic
        traj = arm.plan()

        # 执行规划好的轨迹
        arm.execute(traj)

        rospy.sleep(1)

        # 设置机械臂的目标位置，使用xarm组六个关节的位置数据进行描述（单位：弧度）
        joint_positions = [-0.664, -0.775, 0.675, -1.241, -0.473, -1.281]
        arm.set_joint_value_target(joint_positions)

        # 控制机械臂进行轨迹规划并执行轨迹
        arm.go()

        rospy.sleep(1)

        # 设置手爪规划组gripper里两个关节的位置为0.65，单位弧度
        joint_positions = [0.65, 0.65]
        gripper.set_joint_value_target(joint_positions)

        # 规划并执行，手爪张开
        gripper.go()
        rospy.sleep(1)

        # 设置手爪规划组gripper里两个关节的位置为0，单位弧度
        joint_positions = [0, 0]
        gripper.set_joint_value_target(joint_positions)

        # 规划并执行，手爪闭合
        gripper.go()
        rospy.sleep(1)

        # 设置目标位姿为Home，Home是setup assistant中预先定义好的初始位姿的名称。
        arm.set_named_target('Home')

        # 规划并执行，手臂返回初始位置
        arm.go()

        rospy.sleep(1)

        # 干净地关闭moveit_commander并退出程序
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass
